#include "dm_task.h"
#include "math.h"

void Dm_task(void const * argument)
{
    pid_c_init();
    int i=0;
	DM_target_angle=213;
	osDelay(2000);
	for(i=0;i<1000;i++){
			enable_DM(DM4310_ID1,1);
    enable_DM(DM4310_ID2,1);
	}
	i=0;
	osDelay(200);
    while(1)
    {
                /*rc_ctrl.rc.s[1]==2 || rc_ctrl.rc.s[1]==3  */
			DM_target_pos=angle_to_enc(DM_target_angle);
			DM_target_pos_b=angle_to_enc_b(DM_target_angle_b);
		if(rc_ctrl.rc.s[1]==2 || rc_ctrl.rc.s[1]==3){
			ctrl_motor(DM4310_ID1,0,0,0,0,0);
            ctrl_motor(DM4310_ID2,0,0,0,0,0); 
			//enable_DM(DM4310_ID1,1);
    //enable_DM(DM4310_ID2,1);
				i=0;
        }
        else if(rc_ctrl.rc.s[1]==1)
		{    
//           if(i==0){
//							enable_DM(DM4310_ID1,1);
//					enable_DM(DM4310_ID2,1);
//						 i=1;
//					 }
		    DM_run_pos(DM_target_pos_b,2);
			DM_run_pos(DM_target_pos,1);
			//DM_run_speed();
        }
//        te[0]=DM_run_pos_pid_b.fdb;
//        te[1]=DM_run_pos_pid_b.set;
        // if(timescount<255){
		// 			height=0;
		// 		}
		// 		else if(timescount>=255 && timescount<300){
		// 			height=2;
		// 		}
		// 		else if(timescount>300){
		// 			height=1;
		// 		}
				der_a=der_b;
				der_b=abs(DM_motor_data_b.p_int-angle_to_enc_b(get_re[1]));
				//te[0]=(float)der_b;
        osDelay(1);
    }
}
